/*************************************************************
*
* M3D_MATH.C
*
* Simple math functions
* Solution of quadratic equations
* Transformation of the angles to different ranges
*************************************************************/
#include <stdafx.h>

#include <stdlib.h>
#include <limits.h>

//#include <ap_comm.h>
#include <m3d\m3d.h>

/*************************************************************
* m3dSolveQuadraticEq()
*
* Solve quadratic equation
*
* IN : a, b, c - coefficients of equation
* OUT: root1, root2 - roots of equation (both calculated if nroot > 0)
* RET: number of roots (0-1-2)
*************************************************************/
int m3dSolveQuadraticEq (float a, float b, float c, float *root1, float *root2)
{
   float D;
   float ooa, sqrtD;

   // check that equation is indeed quadratic
   if (m3dIsZero(a)) {
      if (m3dIsZero(b)) {
         return 0;
      } else {
         *root1 = *root2 = -c/b;
         return 1;
      }
   }

   D = m3dPow2(b) - 4.f*a*c;
   if (D < -10*M3D_EPSILON)  {
      // no roots
      return 0;
   }
   D = max(D, 0.f);

   ooa = 1.f / (2.f*a);
   if (D < 10*M3D_EPSILON)  {
      // one root; store both roots to simplify the further processing
      *root1 = *root2 = -b*ooa;
      return 1;
   }

   // two roots
   sqrtD = (float)m3dSqrt(D);
   *root1 = (-b - sqrtD) * ooa;
   *root2 = (-b + sqrtD) * ooa;
   return 2;
}
/******************************************************
* m3dFuncFindZero()
*
* find zero of given function on segment [a, b]
* IN  : f       - function in question
*       pData   - additional data to pass to function 
*       a       - segment begin
*       b       - segment end
*       eps     - result accuracy
* OUT : pA      - found segment begin
*       pB      - found segment end
* RET : TRUE if zero found
******************************************************/
BOOL m3dFuncFindZero (m3dFUNC f, void *pData, float a, float b, float eps, float *pA, float *pB)
{
   BOOL   faNeg;   // TRUE if f(a) < 0
   float  fa, fb;  // f(a) and f(b)
   float  c;       // middle point of segment
   float  fc;      // f(c)
#ifdef _DEBUG
   int    count;   // iteration counter
#endif

   ASSERT(pA != NULL);
   ASSERT(pB != NULL);
   ASSERT(a <= b);
   ASSERT(eps > 0.f);

   // check left end of segment
   fa = f(a, pData);
   if (fa == 0.f) {
      *pA = *pB = a;
      return TRUE;
   }

   // check right end of segment
   fb = f(b, pData);
   if (fb == 0.f) {
      *pA = *pB = b;
      return TRUE;
   }

   // check function signs on ends of segment
   faNeg = fa < 0.f;
   if (faNeg == (fb < 0.f)) {
      return FALSE;
   }

   // locate function zero
#ifdef _DEBUG
   count = 0;
#endif
   while (b - a > eps) {
      c = (a + b) / 2;
      fc = f(c, pData);
      if (fc == 0.f) {
         *pA = *pB = c;
         return TRUE;
      }
      if ((fc < 0.f) == faNeg) {
         a = c;
      } else {
         b = c;
      }
#ifdef _DEBUG
      ++count;
      ASSERT(count < 10000);
#endif
   }

   // store results
   *pA = a;
   *pB = b;
   return TRUE;
}

/******************************************************
* m3dFuncFindMin()
*
* find min of given function on segment [a, b]
* IN  : f       - function in question
*       pData   - additional data to pass to function 
*       a       - segment begin
*       b       - segment end
*       eps     - result accuracy
* OUT : pA      - found segment begin
*       pB      - found segment end
******************************************************/
void m3dFuncFindMin (m3dFUNC f, void *pData, float a, float b, float eps, float *pA, float *pB)
{
   const float   alpha = 0.61803398874989484820458683436564f;
   const float   betta = 0.38196601125010515179541316563436f;
   float         c1, c2;                                        // "middle" points of segment
   float         fc1, fc2;                                      // f(c1), f(c2)
#ifdef _DEBUG
   int    count;   // iteration counter
#endif

   ASSERT(pA != NULL);
   ASSERT(pB != NULL);
   ASSERT(a <= b);
   ASSERT(eps > 0.f);

   c1 = alpha * a + betta * b;
   c2 = betta * a + alpha * b;
   fc1 = f(c1, pData);
   fc2 = f(c2, pData);

   // locate function min
#ifdef _DEBUG
   count = 0;
#endif
   while (b - a > eps) {
      if (fc1 < fc2) {
         b = c2;
         c2 = c1;
         fc2 = fc1;
         c1 = alpha * a + betta * b;
         fc1 = f(c1, pData);
      } else {
         a = c1;
         c1 = c2;
         fc1 = fc2;
         c2 = betta * a + alpha * b;
         fc2 = f(c2, pData);
      }
      ASSERT(a <= c1 && c1 <= c2 && c2 <= b);
      ASSERT(m3dIsZero(c1 - (alpha * a + betta * b)));
      ASSERT(m3dIsZero(c2 - (betta * a + alpha * b)));
#ifdef _DEBUG
      ++count;
      ASSERT(count < 10000);
#endif
   }

   // store results
   *pA = a;
   *pB = b;
   return;
}
/*************************************************************
* m3dUpdateTimeField()
*
* IN : timeField - time field to be updated (subtract elapsedTime)
*      elapsed   - elapsed time
* OUT: timeField - updated
* RET: TRUE  - timeField reached ZERO
*      FALSE - timeField is still greater than zero after subtraction
*
* Subtract 'elapsed' from 'timeField' with clamping
*************************************************************/
BOOL m3dUpdateTimeField (float *timeField, float elapsed)
{
#if (defined _AP_XENON || defined _AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
   // When *timeField is near zero (as it often is in this function),
   // this code is nearly 3x faster than the version below.
   //
   // Verified correct with a little sample:
//   	for (int i = 0; i < 100; i++)
//	{
//		bool ret1 = m3dUpdateTimeField(&time1, elapsed);
//		bool ret2 = m3dUpdateTimeFieldOrig(&time2, elapsed);
//
//		if (ret1 != ret2)
//		{
//			osOutputDebugString("FAILED: time1 = %g ret1 = %d time2 = %g ret2 = %d\n", time1, ret1, time2, ret2);
//		}
//
//		if (time1 != time2)
//		{
//			osOutputDebugString("FAILED: time1 = %g time2 = %g\n", time1, time2);
//		}
//	}

   double tf = *timeField;
   double is_zero = tf - elapsed - M3D_EPSILON;
   
   *timeField = (float)__fsel(is_zero, tf - elapsed, 0);

   tf = __fsel(-*timeField, tf, 0);

   return tf > M3D_EPSILON;

#else
   BOOL rc = FALSE;
   if (*timeField < elapsed + M3D_EPSILON) {
      if (*timeField >= M3D_EPSILON) {
         // return TRUE only first time
         rc = TRUE;
      }
      *timeField = 0.f;
   } else {
      *timeField -= elapsed;
   }
   return rc;
#endif
}

/***************************************************************
* m3dAngleDegTo0_360()
*
* IN:  angle
* RET: angle transformed to the range 0..360
***************************************************************/
float m3dAngleDegTo0_360 (float angle)
{
   float a;

   if (angle >= 0) {
      a = angle - ((int)(angle/360.f)) * 360.f;
   } else {
      a = 360.f + angle - ((int)(angle/360.f)) * 360.f;
   }
   return a;
}

/********************************************************
* m3dAngleDegTo_180_180()
*
* IN:  angle
* RET: angle transformed to the range -180..180
********************************************************/
float m3dAngleDegTo_180_180 (float angle)
{
   float a;

   a = m3dAngleDegTo0_360(angle);
   if (a >= 180.f) {
      a -= 360.f;
   }
   return a;
}
/******************************************************************************
* m3dSolveLinSys2D()
*
* solves 2D liinear system
* IN  : a00, a01, a10, a11 - system matrix
*       b0, b1             - right side of system
* OUT : pX0, pX1           - solution
* RET : TRUE if system has solution (determinant is nonzero), FALSE otherwise
******************************************************************************/
BOOL m3dSolveLinSys2D(float a00, float a01, float a10, float a11, float b0, float b1, float *pX0, float *pX1)
{
   float det;  // system determinant
   float idet; // inverts of determinant

   ASSERT(pX0 != NULL && pX1 != NULL);

   det = a00 * a11 - a01 * a10;
   if (m3dIsZero(det)) {
      return FALSE;
   }
   idet = 1.f / det;
   *pX0 = (b0 * a11 - a01 * b1) * idet;
   *pX1 = (a00 * b1 - b0 * a10) * idet;

   //ASSERT(m3dIsZero_A(a00 * *pX0 + a01 * *pX1 - b0, 1000.f * M3D_EPSILON));
   //ASSERT(m3dIsZero_A(a10 * *pX0 + a11 * *pX1 - b1, 1000.f * M3D_EPSILON));
   return TRUE;
}

/*************************************************************
* m3dLimitChange_Float()
*
*************************************************************/
BOOL m3dLimitChange_Float (float src, float dest, float maxLen, float *out)
{
   BOOL rc = FALSE;

   ASSERT(maxLen > 0.f);

   if (dest > src) {
      *out = src + maxLen;
      if (*out >= dest) {
         *out = dest;
         rc = TRUE;
      }
   } else {
      *out = src - maxLen;
      if (*out <= dest) {
         *out = dest;
         rc = TRUE;
      }
   }

   return rc;
}

/************************************************************
* m3dIsPower2()
*
************************************************************/
BOOL m3dIsPower2 (int x)
{
   if ( x == 0 ) return FALSE;
   return ((x-1) & x) == 0;
}


/************************************************************
* m3dNearestGEPower2()
* returns nearest greater or equal power of 2 for 'x'
*
* Remarks:
* In 12 operations, this code computes the next highest power 
* of 2 for a 32-bit integer. The result may be expressed by 
* the formula 1 << (lg(v - 1) + 1). Note that in the edge case 
* where v is 0, it returns 0, which isn't a power of 2; you 
* might append the expression v += (v == 0) to remedy this if 
* it matters. It works by copying the highest set bit to all 
* of the lower bits, and then adding one, which results in 
* carries that set all of the lower bits to 0 and one bit beyond 
* the highest set bit to 1. If the original number was a power 
* of 2, then the decrement will reduce it to one less, so 
* that we round up to the same original value. 
************************************************************/
int m3dNearestGEPower2(int x)
{
   CASSERT(sizeof(int) == 4);

   x--;
   x |= x >> 1;
   x |= x >> 2;
   x |= x >> 4;
   x |= x >> 8;
   x |= x >> 16;
   x++;
   return x;
}

/************************************************************
* m3dSaw()
*
************************************************************/
float m3dSaw (float period, float t) 
{
   ASSERT(t >= 0.f && t < period);
   if (t < period/4.f) {
      return m3dLerp(0.f, 1.f, 0.f, period/4.f, t);
   } else if (t > 3.f*period/4.f) {
      return m3dLerp(-1.f, 0.f, 3.f*period/4.f, period, t);
   }
   return m3dLerp(1.f, -1.f, period/4.f, 3.f*period/4.f, t);
}

//
// End-of-file M3D_MATH.CPP
//
